#include <opencv2/core.hpp>
#include <opencv2/imgproc.hpp>
#include <opencv2/imgcodecs.hpp>
#include <opencv2/highgui.hpp>
#include <iostream>
#include <algorithm>
#include <vector>
#include "Fast.h"
#include <opencv2/features2d/features2d.hpp>

using namespace std;
using namespace cv;


using namespace std;
void makeAgastOffsets(int pixel[16], int rowStride, int type)
{
  static const int offsets16[][2] =
  {
    {-3,  0}, {-3, -1}, {-2, -2}, {-1, -3}, {0, -3}, { 1, -3}, { 2, -2}, { 3, -1},
    { 3,  0}, { 3,  1}, { 2,  2}, { 1,  3}, {0,  3}, {-1,  3}, {-2,  2}, {-3,  1}
  };

  static const int offsets8[][2] =
  {
    {-1,  0}, {-1, -1}, {0, -1}, { 1, -1},
    { 1,  0}, { 1,  1}, {0,  1}, {-1,  1}
  };

  const int (*offsets)[2] = type == nOAST_9_16 ? offsets16 :
                            type == nAGAST_5_8 ? offsets8  : 0;


  int k = 0;
  for( ; k < 16; k++ )
    pixel[k] = offsets[k][0] + offsets[k][1] * rowStride;
}



int agast_cornerScore_9_16_(const unsigned char* ptr, const int pixel[], int threshold)
{
  int bmin = threshold;
  int bmax = 255;
  int b_test = (bmax + bmin) / 2;
  register short offset[16] = { 0 };
  offset[0]= (short)pixel[0];
  offset[1]= (short)pixel[1];
  offset[2]= (short)pixel[2];
  offset[3]= (short)pixel[3];
  offset[4]= (short)pixel[4];
  offset[5]= (short)pixel[5];
  offset[6]= (short)pixel[6];
  offset[7]= (short)pixel[7];
  offset[8]= (short)pixel[8];
  offset[9]= (short)pixel[9];
  offset[10] = (short)pixel[10];
  offset[11] = (short)pixel[11];
  offset[12] = (short)pixel[12];
  offset[13] = (short)pixel[13];
  offset[14] = (short)pixel[14];
  offset[15] = (short)pixel[15];

  while (true)
  {
    register const int cb = *ptr + b_test;
    register const int c_b = *ptr - b_test;
    int flag[32] = { 0 };
    for (int k = 0; k < 16; k++)
    {
      if (ptr[offset[k]] > cb)
      {
        flag[k] = 1;
        flag[k + 16] = 1;

      }
      else if (ptr[offset[k]] < c_b)
      {
        flag[k] = -1;
        flag[k + 16] = -1;
      }
      else
      {
        flag[k] = 0;
        flag[k + 16] = 0;
      }

    }
    int temp = 0;
    int count = 0;
    for (int k = 0; k < 31; k++)
    {
      temp = flag[k] * flag[k + 1];
      if (temp > 0)
      {
        count++;
      }
      else
      {
        count = 0;
      }
      if (count >= 8)
      {
        goto is_a_corner;
      }
    }
    goto is_not_a_corner;

    is_a_corner:
      bmin = b_test;
      goto end;

    is_not_a_corner:
      bmax = b_test;
      goto end;

    end:
      if (bmin == bmax - 1 || bmin == bmax)
        return bmin;
      b_test = (bmin + bmax) / 2;
  }
}

int agast_cornerScore_5_8_(const unsigned char* ptr, const int pixel[], int threshold)
{
  int bmin = threshold;
  int bmax = 255;
  int b_test = (bmax + bmin) / 2;


  register short offset[8] = { 0 };
  offset[0] = (short)pixel[0];
  offset[1] = (short)pixel[1];
  offset[2] = (short)pixel[2];
  offset[3] = (short)pixel[3];
  offset[4] = (short)pixel[4];
  offset[5] = (short)pixel[5];
  offset[6] = (short)pixel[6];
  offset[7] = (short)pixel[7];

  while (true)
  {
    register const int cb = *ptr + b_test;
    register const int c_b = *ptr - b_test;
    int flag[16] = { 0 };
    for (int k = 0; k < 8; k++)
    {
        if (ptr[offset[k]] > cb)
        {
          flag[k] = 1;
          flag[k + 8] = 1;

        }
        else if (ptr[offset[k]] < c_b)
        {
          flag[k] = -1;
          flag[k + 8] = -1;
        }
        else
        {
          flag[k] = 0;
          flag[k + 8] = 0;
        }

    }
    int temp = 0;
    int count = 0;
    for (int k = 0; k < 15; k++)
    {
        temp = flag[k] * flag[k + 1];
        if (temp > 0)
        {
          count++;
        }
        else
        {
          count = 0;
        }
        if (count >= 4)
        {
          goto is_a_corner;
        }
    }
    goto is_not_a_corner;

    is_a_corner:
      bmin = b_test;
      goto end;

    is_not_a_corner:
      bmax = b_test;
      goto end;

    end:
      if (bmin == bmax - 1 || bmin == bmax)
          return bmin;
      b_test = (bmin + bmax) / 2;
  }
}


void OAST_9_16_(unsigned char * img, int mWidth, int mHeight, std::vector<OneKeyPoint>& keypoints, int threshold)
{
  size_t total = 0;
  int xsize = mWidth;
  int ysize = mHeight;
  size_t nExpectedCorners = keypoints.capacity();
  register int x, y;
  register int xsizeB = xsize - 3;
  register int ysizeB = ysize - 3;
  register int width;

  keypoints.resize(0);

  int pixel_9_16_[16];
  makeAgastOffsets(pixel_9_16_, xsize, nOAST_9_16);
  register short offset[16] = { 0 };
  offset[0] = (short)pixel_9_16_[0];
  offset[1] = (short)pixel_9_16_[1];
  offset[2] = (short)pixel_9_16_[2];
  offset[3] = (short)pixel_9_16_[3];
  offset[4] = (short)pixel_9_16_[4];
  offset[5] = (short)pixel_9_16_[5];
  offset[6] = (short)pixel_9_16_[6];
  offset[7] = (short)pixel_9_16_[7];
  offset[8] = (short)pixel_9_16_[8];
  offset[9] = (short)pixel_9_16_[9];
  offset[10] = (short)pixel_9_16_[10];
  offset[11] = (short)pixel_9_16_[11];
  offset[12] = (short)pixel_9_16_[12];
  offset[13] = (short)pixel_9_16_[13];
  offset[14] = (short)pixel_9_16_[14];
  offset[15] = (short)pixel_9_16_[15];

  width = xsize;
  int flag[32] = { 0 };
  for (y = 3; y < ysizeB; y+=1)
  {
    for (x = 3; x < xsizeB; x+=1)
    {
      register const unsigned char* const ptr = img + y*width + x;
      register const int cb = *ptr + threshold;
      register const int c_b = *ptr - threshold;
      for (int k = 0; k < 16; k++)
      {
        if (ptr[offset[k]] > cb)
        {
          flag[k] = 1;
          flag[k + 16] = 1;

        }
        else if (ptr[offset[k]] < c_b)
        {
          flag[k] = -1;
          flag[k + 16] = -1;
        }
        else
        {
          flag[k] = 0;
          flag[k + 16] = 0;
        }

      }
      int temp = 0;
      int count = 0;
      for (int k = 0; k < 31; k++)
      {
        temp = flag[k] * flag[k + 1];
        if (temp > 0)
        {
          count++;
        }
        else
        {
          count = 0;
        }
        if (count >= 8)
        {
          OneKeyPoint tempKeyPoint;
          tempKeyPoint.pt.x = (float)x;
          tempKeyPoint.pt.y = (float)y;
          tempKeyPoint.size = 1.0f;
          tempKeyPoint.response = (float)agast_cornerScore_9_16_(ptr, pixel_9_16_, threshold);

          keypoints.push_back(tempKeyPoint);
          //printf("x = %d, y = %d\n", x, y);
          break;
        }
      }

    }

  }
}



Fast::Fast(){

  Mat img, ImGray, Imgme, ImgO;
  float sum;
  int nRows, nCols;
  string imgPath = "../pic/1.jpg";
  string GimgPath = "../pic/GimgR.jpg";
  string OimgPath = "../pic/Oimg.jpg";
	
  /* 读取图片 */
  img = imread(imgPath);
  Imgme = img.clone();
  ImgO = img.clone();
  cvtColor(img,ImGray,cv::COLOR_BGR2GRAY);

  nRows = img.rows;
	nCols = img.cols;
  cout << "read image ok." << "  width: " << nRows << "  heigh: " << nCols << endl;

  vector<OneKeyPoint> keypoints;
  int threshold = 30;
  int64 t1 = cv::getTickCount();
  OAST_9_16_(ImGray.data, ImGray.cols, ImGray.rows, keypoints, threshold);
  int64 t2 = cv::getTickCount();
  cv::Point2f srcPts;
  for (int k = 0; k < keypoints.size(); k++) {

    srcPts.x = keypoints[k].pt.x;
    srcPts.y = keypoints[k].pt.y;
    circle(Imgme, srcPts, 1, Scalar(0, 255, 0));
  }
  cout << "Fast detect ok" << endl;
  
  
  vector<cv::KeyPoint> vKeysCell;
  int64 t3 = cv::getTickCount();
  FAST(ImGray,vKeysCell,threshold,true);
  int64 t4 = cv::getTickCount();
  cv::Point2f srcPts1;
  for (int k = 0; k < vKeysCell.size(); k++) {

    srcPts1.x = vKeysCell[k].pt.x;
    srcPts1.y = vKeysCell[k].pt.y;
    circle(ImgO, srcPts1, 1, Scalar(0, 255, 0));
  }

  /* 保存图片 */
  imwrite(GimgPath, Imgme);
  imwrite(OimgPath, ImgO);
  cout << "save Fast img ok." << endl;

  float t_OAST_9_16_ = 1000 * (t2 - t1) / cv::getTickFrequency();
  float t_FAST = 1000 * (t4 - t3) / cv::getTickFrequency();
  std::cout << "t_OAST_9_16_: " << t_OAST_9_16_ << "   t_FAST:  " << t_FAST << std::endl ;

std::cout << "t_OAST_9_16_ Keypoints " << keypoints.size() << std::endl;
std::cout << "FAST Keypoints " << vKeysCell.size() << std::endl;
}


Fast::~Fast(void){
  cout << "byby ~~~ I'm jianbao" << endl;
}




